#include <matching.h>

static bool comp_ascend(pair<P2d,P2d> &p1, pair<P2d,P2d> &p2)
{
  return ((p1.first < p2.first) ? true : false);
}

void convertflowPoint2P2d(vector<Point2f> flowpoint, vector<Point2f> flowscale, vector< pair<P2d,P2d> > *vals)
{
  if(vals->size() > 0)
    vals->clear();

  if(flowpoint.size() != flowscale.size())
    {
      std::cerr << "flowPoint is different from flowScale" << std::endl;
      exit(1);
    }

  for(uint i=0; i<flowpoint.size(); i++)
    {
      float x = flowpoint[i].x;
      float y = flowpoint[i].y;
      float flen = hypot(flowscale[i].x, flowscale[i].y);
      float ang = atan2(flowscale[i].y, flowscale[i].x);
      vals->push_back( pair<P2d,P2d>(P2d(x,y),P2d(flen,ang)) );
    }
}

void convertP2d2flowPoint(vector< pair<P2d,P2d> > vals, vector<Point2f> *flowPoint, vector<Point2f> *flowScale)
{
  if(flowPoint->size() > 0)
    flowPoint->clear();
  if(flowScale->size() > 0)
    flowScale->clear();

  for(uint i = 0; i<vals.size(); i++)
    {
      Point2f flowP,flowS;
      flowP.x = vals[i].first.first;
      flowP.y = vals[i].first.second;
      flowS.x = vals[i].second.first * cos(vals[i].second.second);
      flowS.y = vals[i].second.first * sin(vals[i].second.second);
      flowPoint->push_back(flowP);
      flowScale->push_back(flowS);
    }
}


int simpleRansac(const vector< pair<P2d,P2d> > vals,float result[2][2])
{
  uint pnum = vals.size(), samplenum = pnum/3; //サンプリング点の設定 tekitou!!
  float bestvar = std::numeric_limits<float>::max();
  list< pair<P2d,P2d> > vlist;

  // if(pnum == 0)
  //   {
  //     cout << "vals is empty" << endl;
  //     exit(0);
  //   }
  if(pnum < 5)
    {
      cout << "flows are  not enough to compare" << endl;
      return -1;
    }

  for(int i=0; i<30; i++) // the number of trial (１０じゃ少ないはず)
    {

      //サンプリング（重複しないように）
      while(1)
        {
	  int n = cvRandInt(&rng_state)%pnum;
	  vlist.push_back(vals.at(n));

          if(vlist.size() == samplenum)
            {
              vlist.sort(comp_ascend);
              vlist.unique();

              //uniqueした上で、要素数がサンプル数と同じになっていればサンプリングは完了
	      if(vlist.size() == samplenum)
		break;
            }
        }


      //サンプリングした点の平均を求める
      float ave1=0, ave2=0;
      list< pair<P2d,P2d> >::iterator it = vlist.begin();
      for(; it != vlist.end(); it++)
        {
          ave1 += it->second.first;
          ave2 += it->second.second;
        }
      ave1 /= (float)samplenum;
      ave2 /= (float)samplenum;

      //サンプリングした点で分散値を計算
      float var1=0,var2=0;
      it = vlist.begin();
      for(; it != vlist.end(); it++)
        { 
          var1 += pow(it->second.first  - ave1, 2);
          var2 += pow(it->second.second - ave2, 2);
        }
      var1 /= (float)samplenum;
      var2 /= (float)samplenum;

      float var = hypot(var1, 4*var2); //なんとなくスケールを合わせる tekitou!!
      if(var < bestvar)
        {
          bestvar = var;
          result[0][0] = ave1;
          result[0][1] = ave2;
          result[1][0] = var1;
          result[1][1] = var2;
        }
      vlist.clear();
    }
  return 0;
}

void selectPoints(const vector< pair<P2d,P2d> > vals, float param[2][2],
                       vector< pair<P2d,P2d> > *selected)
{
  vector< pair<P2d,P2d> >::const_iterator it = vals.begin();
  for(; it != vals.end(); it++)
    {
      const P2d *p = &(it->second);
      float d1 = pow(p->first  - param[0][0],2);
      float d2 = pow(p->second - param[0][1],2);
      if(d1 < param[1][0] && d2 < param[1][1])
        selected->push_back(*it);
    }
}

int fileFlowData(int imcount,
		 const vector< pair<P2d,P2d> > vals,
		 const vector< pair<P2d,P2d> > selected)
{
  char filename[128];
  sprintf(filename,"data/all_flow/allflow_%04d.dat",imcount);
  FILE *fp = fopen(filename,"w");
  vector< pair<P2d,P2d> >::const_iterator it = vals.begin();
  for(; it != vals.end(); it++)
    fprintf(fp,"%f %f\n",it->second.first,it->second.second);
  fclose(fp);

  sprintf(filename,"data/selected_flow/selectedflow_%04d.dat",imcount);
  fp = fopen(filename,"w");
  it = selected.begin();
  for(; it != selected.end(); it++)
    fprintf(fp,"%f %f\n",it->second.first,it->second.second);
  fclose(fp);

  sprintf(filename,"data/dst_img/flowimage_%04d.ppm",imcount);
  return 0;
}


int countPic(const cv::Mat input)
{
  int x,y;
  int p[3];
  int p_sum = 0;
  int count = 0;
  if(input.channels() == 3)
    {
      for (y = 0; y < input.rows; y++)
	{
	  for (x = 0; x < input.cols; x++)
	    {
	      p[0] = input.data[input.step * y + x * 3];        // B
	      p[1] = input.data[input.step * y + x * 3 + 1];    // G
	      p[2] = input.data[input.step * y + x * 3 + 2];    // R
	      p_sum = p[0] + p[1] + p[2];
	      if ( p_sum > 0)
		{
		  count++;
		}
	      p_sum = 0;
	    }
	}
      return count;
    }
  else if(input.channels() == 1)
    {
      for (y = 0; y < input.rows; y++)
	{
	  for (x = 0; x < input.cols; x++)
	    {
	      p[0] = input.data[input.step * y + x * 3];
	      if ( p[0] > 0)
		count++;
	    }
	}
      return count;
    }
  return 0;
}

void calcImageCenter(const cv::Mat input, Point2f *center)
{
  int x,y;
  int p[3];
  int p_sum = 0;
  int count = 0;
  center->x = 0.0;
  center->y = 0.0;
  if(input.channels() == 3)
    {
      for (y = 0; y < input.rows; y++)
	{
	  for (x = 0; x < input.cols; x++)
	    {
	      p[0] = input.data[input.step * y + x * 3];        // B
	      p[1] = input.data[input.step * y + x * 3 + 1];    // G
	      p[2] = input.data[input.step * y + x * 3 + 2];    // R
	      p_sum = p[0] + p[1] + p[2];
	      if ( p_sum > 0)
		{
		  count++;
		  center->x += (float)x;
		  center->y += (float)y;
		}
	      p_sum = 0;
	    }
	}
    }
  else if(input.channels() == 1)
    {
      for (y = 0; y < input.rows; y++)
	{
	  for (x = 0; x < input.cols; x++)
	    {
	      p[0] = input.data[input.step * y + x * 3];
	      if ( p[0] > 0){
		count++;
		center->x += (float)x;
		center->y += (float)y;
	      }
	    }
	}
    }
  center->x /= (float)count;
  center->y /= (float)count;
}

float calcMode(list<float> point, int num)
{
  float max,min;
  float step;
  vector<int> count;
  list<float>::iterator min_it = min_element( point.begin(), point.end());
  list<float>::iterator max_it = max_element( point.begin(), point.end());

  min = *min_it;
  max = *max_it;
  step = (max - min + 1)/num;

  for(int i = 0 ; i < num ; i++){
    int temp = 0;
    count.push_back(temp);
  }

  for(std::list<float>::iterator i = point.begin(); i != point.end(); i++)
    {
      for(int j = 0 ; j < num-1 ; j++){
        if(min+step*j <= (*i) && min+step*(j+1) > (*i)){
          count[j]++;
        }
      }
      if(min+step*(num-1) <= (*i))
        count[num-1]++;
    }

  int max_index=0;
  for(int i = 1 ; i<num ; i++){
    if(count[max_index]<count[i]){
      max_index = i;
    }
  }

  return ((min+step*max_index + min+step*(max_index + 1))*0.5);
}

void calcFlowMode(vector<Point2f> input, vector<Point2f> *output, int num)
{
  list<float> point;
}

float calcMean(list<float> point)
{
  float sum = 0;
  int count = 0;

  for(std::list<float>::iterator i = point.begin(); i != point.end(); i++)
    {
      sum += (*i);

      count++;

    }

  return sum / count ;
}

void calcMeanVar(vector< pair<P2d,P2d> > features, pair<P2d,P2d> *mean, pair<P2d,P2d> *var)
{

  //サンプリングした点の平均を求める
  mean->first.first = 0;
  mean->first.second = 0;
  mean->second.first = 0;
  mean->second.second = 0;


  std::vector< pair<P2d,P2d> >::iterator it = features.begin();
  for(; it != features.end(); it++)
    {
      mean->first.first += it->first.first;
      mean->first.second += it->first.second;
      mean->second.first += it->second.first;
      mean->second.second += it->second.second;

    }
  mean->first.first /= (float)features.size();
  mean->first.second /= (float)features.size();
  mean->second.first /= (float)features.size();
  mean->second.second /= (float)features.size();

  //サンプリングした点で分散値を計算
  var->first.first = 0;
  var->first.second = 0;
  var->second.first = 0;
  var->second.second = 0;

  it = features.begin();
  for(; it != features.end(); it++)
    {
      var->first.first += pow(it->first.first  - mean->first.first, 2);
      var->first.second += pow(it->first.second - mean->first.second, 2);
      var->second.first += pow(it->second.first  - mean->second.first, 2);
      var->second.second += pow(it->second.second - mean->second.second, 2);
    }
  var->first.first /= (float)features.size();
  var->first.second /= (float)features.size();
  var->second.first /= (float)features.size();
  var->second.second /= (float)features.size();

}


cv::Mat makeMask(cv::Mat input)
{
  cv::Mat dst_img;
  cvtColor(input, dst_img, CV_RGB2GRAY);
  threshold(dst_img, dst_img, 1, 255, THRESH_BINARY);
  return dst_img;
}

cv::Mat loadImage(std::string dirname, int imagenum)
{
  cv::Mat capture;
  std::stringstream str;
  str << dirname.c_str() <<"frame" << setw(4) << setfill('0') << imagenum << ".ppm";
  capture = imread(str.str().c_str());
  if(capture.cols == 0 || capture.rows == 0)
    {
      cout << "load error, enpty file" << endl;
      cout << str.str().c_str() << endl;
    }
  return capture;

}

cv::Mat boxedMask(cv::Mat input, int step)
{
  cv::Mat mask = makeMask(input);
  cv::Mat output = Mat::zeros(input.rows, input.cols, CV_32F);

  for(int i = 0; i < mask.cols; i += step){
    for(int j = 0; j < mask.rows; j += step){
      int count_pic = 0;
      for(int k = 0; k < step ; k++){
	for(int n = 0; n < step ; n++){
	  if(mask.data[mask.cols * (j + n) + (i + k)] != 0){
	    count_pic++;
	  }
	}

      }

      if(count_pic > pow(step, 2)*0.5){
        rectangle( output, Point(i,j), Point(i+step,j+step),Scalar(255,255,255),-1);
      }
    }
  }
  return output;
}

void boxedFlow(std::vector<Point2f> flowPoint, std::vector<Point2f> flowScale, std::vector<Point2f> *boxedflowPoint, std::vector<Point2f> *boxedflowScale, int step, cv::Mat input)
{
  if(boxedflowPoint->size() > 0)
    boxedflowPoint->clear();

  if(boxedflowScale->size() > 0)
    boxedflowScale->clear();

  if(flowPoint.size() != flowScale.size())
    {
      std::cerr << "flowPoint is different from flowScale" << std::endl;
      exit(1);
    }

  for(int i = 0; i < input.cols; i += step)
    {
      for(int j = 0; j < input.rows; j += step)
	{

	  Point2f flow(0,0);
	  int count_flow = 0;
	  for(uint k = 0; k < flowPoint.size(); k++){
	    if(flowPoint[k].x >= i && flowPoint[k].x < i + step )
	      {
		if(flowPoint[k].y >= j && flowPoint[k].y < j + step )
		  {
		    flow.x += flowScale[k].x;
		    flow.y += flowScale[k].y;
		    count_flow++;

		  }
	      }
	  }
	  if (count_flow != 0)
	    {
	      flow.x /= count_flow;
	      flow.y /= count_flow;
	      boxedflowPoint->push_back(Point2f(i+step*0.5, j+step*0.5));
	      boxedflowScale->push_back(flow);
	    }
	}
    }
}

void extractRangeFlow(vector<Point2f> flowPoint, vector<Point2f> flowScale, vector<Point2f> *resultFlowPoint, vector<Point2f> *resultFlowScale, float  maxshre, float  minshre)
{
  if(flowPoint.size() != flowScale.size())
    {
      std::cerr << "flowPoint is different from flowScale" << std::endl;
      exit(1);
    }
  if(maxshre == -1)
    maxshre = std::numeric_limits<float>::max();
  if(minshre == -1)
    minshre = std::numeric_limits<float>::min();
  if(resultFlowPoint->size() > 0)
    resultFlowPoint->clear();
  if(resultFlowScale->size() > 0)
    resultFlowScale->clear();

  for(uint i = 0; i<flowPoint.size(); i++)
    {
      float x = flowPoint[i].x;
      float y = flowPoint[i].y;
      float flow_x = flowScale[i].x;
      float flow_y = flowScale[i].y;
      float flen = hypot(flow_x, flow_y);
      if(flen < maxshre)
	{
	  if(flen > minshre)
	    {
	      resultFlowPoint->push_back(Point2f(x,y));
	      resultFlowScale->push_back(Point2f(flow_x,flow_y));
	    }
	}
    }
}

int saveFlowData(vector<Point2f> flowPoint, vector<Point2f> flowScale, int imnum)
{
  vector< pair<P2d,P2d> > vals;
  vector< pair<P2d,P2d> > selected;
  convertflowPoint2P2d(flowPoint, flowScale, &vals);

  if(vals.size() < 10)
    {
      printf("Not enough flows for save all flow");
      return 1;
    }
  else
    {
      float param[2][2];
 
      simpleRansac(vals,param);
      selectPoints(vals,param,&selected);
      if(selected.size() < 10)
	{
	  printf("Not enough flows for save selected flow");
	  return 1;
	}
      fileFlowData(imnum, vals, selected);
      selected.clear();
    }
  return 0;
}

void firstProc(std::string dirname, int imcount, cv::Mat& output)
{

  //pwdとhomeの設定
  static char *home = getenv("HOME");
  //static char *pwd;
  //getcwd(pwd, sizeof(pwd));

  //ターゲットの設定
  string imdir;
  imdir.append(home);
  imdir.append("/Pictures/");
  imdir.append(dirname.c_str());
  imdir.append("/");

  output = loadImage(imdir, imcount);
}


void display(int cnum, cv::Mat input, cv::Mat &output, vector< pair<P2d,P2d> > flow, char* windowname)
{
  output = input.Mat::clone();
  if(cnum > ColorGroupNum)
    {
      std::cerr << "until 16 colors" << std::endl;
      exit(1);
    }
  for(uint i = 0; i < flow.size(); i++)
    {

      int x = flow[i].first.first;
      int y = flow[i].first.second;
      float leng = flow[i].second.first;
      float ang = flow[i].second.second;
      int n = (int)((float)cnum*(ang + M_PI)/(2*M_PI));
      cv::circle(output, Point(x, y), 1, colorlist[n]);
      cv::line(output, Point(x - leng*cos(ang),y - leng*sin(ang)),
	       Point(x,y), colorlist[n]);

    }
  imshow(windowname, output);

}

void calcMaxScale(std::vector<Point2f> flowScale, float &maxScale)
{

  maxScale = 0.0;
  for(uint i = 0; i < flowScale.size(); i++)
    {
      double x = flowScale[i].x;
      double y = flowScale[i].y;

      float scale = sqrt(pow(x,2) + pow(y,2));
      if(maxScale < scale)
	maxScale = scale;
    }
}

cv::Mat genFlowHist(std::vector<Point2f> flowScale, std::vector<Point2f> flowPoint, cv::Mat input)
{
  if(flowPoint.size() != flowScale.size())
    {
      std::cerr << "flowPoint is different from flowScale" << std::endl;
      exit(1);
    }

  cv::Mat hist_img = Mat::zeros(input.rows, input.cols, CV_8U);
  float maxScale;
  calcMaxScale(flowScale, maxScale);

  for( uint i = 0; i < flowScale.size(); i++)
    {
      int x = flowPoint[i].x;
      int y = flowPoint[i].y;
      double scale_x = flowScale[i].x;
      double scale_y = flowScale[i].y;
      float scale = sqrt(pow(scale_x,2)+pow(scale_y,2));
      rectangle(hist_img, Point(x-5,y-5), Point(x+5,y+5),Scalar(255*scale/maxScale),-1);

    }
  return hist_img;
}

void calcDnsArea(std::vector<Point2f> flowPoint, std::vector<Point2f> flowScale, int step, cv::Mat input, float thre, cv::Mat &maskImage)
{

  vector<vector<float> > box;
  for(int i = 0; i < input.cols+step; i += step){
    vector<float> temp;
    for(int j = 0; j < input.rows+step; j += step){
      temp.push_back(0.0);
    }
    box.push_back(temp);
  }
  vector<Point2f> boxedPoint, boxedScale;
  boxedFlow(flowPoint, flowScale, &boxedPoint, &boxedScale, step, input);
  for(uint i = 0; i < boxedPoint.size(); i++){
    int px = boxedPoint[i].x/step;
    int py = boxedPoint[i].y/step;
    float fx = boxedScale[i].x;
    float fy = boxedScale[i].y;

    if(fx != 0){
      box[px][py] -= fabs(fx);
      if(fx > 0){
	if((uint)(px + 1) != box.size())
	  box[px + 1][py] += fx;
      }else{
	if(px != 0)
	  box[px - 1][py] += -fx;
      }
    }
    if(fy != 0){
      box[px][py] -= fabs(fy);
      if(fy > 0){
	if((uint)(py + 1) != box[px].size())
	  box[px][py + 1] += fy;
      }else{
	if(py != 0)
	  box[px][py - 1] += -fy;
      }
    }
  }



  maskImage = Mat::zeros(input.rows, input.cols, CV_8UC3);
  //maskImage = Mat::zeros(input.rows, input.cols, CV_8UC1);
  for(uint i = 0; i< box.size(); i++){
    for(uint j = 0; j< box[i].size(); j++){
      if(box[i][j] > thre){
	rectangle( maskImage, Point(i*step, j*step),
		   Point((i+1)*step, (j+1)*step), Scalar(0,0,255), -1);

      }else if(box[i][j] < -thre){
	rectangle( maskImage, Point(i*step, j*step),
		   Point((i+1)*step, (j+1)*step), Scalar(255,0,0), -1);

      }
    }
  }
}

void resizeSrcImg(cv::Mat input, cv::Mat &output, float scale = 1.0, int interpolation = INTER_LINEAR )
{
  cv::Mat temp;

  input.copyTo(temp);

  //output = Mat::create(cvRound(input.rows*scale), cvRound(input.cols*scale), CV_32FC3);
  resize(temp, output, Size(), scale, scale, interpolation);

}
